﻿/* ------------------------------------------------------------------------- */
/*
 * C O M P A N Y   P R O P R I E T A R Y
 * use or disclosure of data contained in this file is subject to the
 * requirements an the end of this file
 *
 * CLASSIFICATION: OFFEN / UNCLASSIFIED
 */
/* ------------------------------------------------------------------------- */

/* ######################################################################### */
/** \file M44_ConfidenceAssignmet.cs
 * \brief	This file contains the ConfidenceAssignment function that estimates 
 *          the estimation confidence of each cell of the map. 
 *  
 * This file contains the ConfidenceAssignment function that estimates 
 * the confidence of each cell of the map. 
 * 
 * 
 * PROJEKT:   NETWORKED MULTI-ROBOT SYSTEMS
 *
 * Copyright (c) 2008 SENER - Universidad Politécnica de Madrid.
 *
 * \author    Antonio Matta \n
 *            Universidad Politécnica de Madrid \n
 *            Grupo de Robótica y Cibernética \n
 * \version   0.03
 * \date      2010-02-04
 *
 * \n \n
 * Versionshistory: \n
 * -----------------
 * - Version 0.01:   Antonio MATTA                          2009-09-01 \n
 *      First implementation of the ConfidenceAssignment function.
 *      
 * - Version 0.02:   Antonio MATTA                          2009-09-28 \n
 *      QuadsNumber function modified.
 *      
 * - Version 0.03: Thomas KOPFSTEDT                         2010-02-04 \n
 *   changes of class names due to cleanup in "interfaces.cs", and the 
 *   other interface classed to double names for the same class
 *   
 * - Version 0.04: Antonio MATTA                         2010-02-26 \n
 *   unused code removed.
 */
/* ######################################################################### */

using System;
using System.Collections.Generic;
using System.Text;

/*Using the C Sharp Matrix Library*/
using NMRS.CSML;

/*Using Interfaces classes*/
using Interfaces;
using Interfaces.Complex;
using Interfaces.Basis;

/*This is for using lists*/
using System.Collections;

/* ------------------------------------------------------------------------- */
/**	\namespace NMRS.M44_SensorFusionAndMapping
 *
 *  \brief	This is the namespace of the M44 Sensor Fusion and Mapping module
 */
/* ------------------------------------------------------------------------- */
namespace NMRS.M44_SensorFusionAndMapping
{
    /* ------------------------------------------------------------------------- */
    /**	\class M44_ConfidenceAssignmet
     *
     *  \brief  This is the class of the M44_ConfidenceAssignmet function.
     * 
     */
    /* ------------------------------------------------------------------------- */
    static class M44_ConfidenceAssignmet
    {
        /* ------------------------------------------------------------------------- */
        /**	public static aux_MapCell ConfidenceAssignment( aux_MapCell aux_MapCell_local,
         *                                                  float tf_3DLRFRange,
         *                                                  ArrayList[,] Cell_List,
         *                                                  cl_Map2D HighResMap
         *                                                 )
         * 
         *  \brief Function that calculates the cell confidence.
         *  
         *  We consult the times we mark a cell as free and occupied.
         *  We penalize the measures taken in a far away distance from de robot. 
         *  With this procedure we try to reduce the noise
         *  effect produced by angular measure error, especially in the far distances
         *  
         *  \param[in] aux_MapCell aux_MapCell_local
         *  \param[in] float tf_3DLRFRange
         *  \param[in] ArrayList[,] Cell_List
         *  \param[in] cl_Map2D HighResMap
         *
         */
        /* ------------------------------------------------------------------------- */
        public static aux_MapCell ConfidenceAssignment(aux_MapCell aux_MapCell_local,
                                                        float tf_3DLRFRange,
                                                        ArrayList[,] Cell_List,
                                                        cl_Map2D HighResMap
                                                       )
        {
            //float d;
            byte[,] Quads_Matrix;
            Quads_Matrix = new byte[HighResMap.us_Nrows, HighResMap.us_Ncols];
            

            /* The first values of confidence layer are the 
             * number of quads occuped in each cell
             */
            //aux_MapCell_local.Confidence = QuadsNumber(Cell_List, HighResMap);
            Quads_Matrix = QuadsNumber(Cell_List, HighResMap);
            

            /* Now a filter based on measured distance is 
             * applied to dimish the confidence */
            
            for (int i = 0; i < aux_MapCell_local.us_Nrows; i++)
            {
                for (int j = 0; j < aux_MapCell_local.us_Ncols; j++)
                {
                    aux_MapCell_local.Confidence[i, j] = (byte)(Quads_Matrix[i, j]); 
                }
            }
            
            return aux_MapCell_local;
        }

        /* ------------------------------------------------------------------------- */
        /**	public static float[,] QuadsNumber(  ArrayList[,] Cell_List,
         *                                      cl_Map2D HighResMap
         *                                    )
         * 
         *  \brief 
         *  
         *  
         *  \param[in] ArrayList[,] Cell_List
         *  \param[in] cl_Map2D HighResMap
         *
         */
        /* ------------------------------------------------------------------------- */
        public static byte[,] QuadsNumber(ArrayList[,] Cell_List,
                                            cl_Map2D HighResMap
                                          )
        {
            int[] i_CellPos = new int[2]; ;
            byte[,] Quads_Matrix;
           
            float[] lim = new float[2];
            float r;
            float x;
            float y;
            
            Quads_Matrix = new byte[HighResMap.us_Nrows, HighResMap.us_Ncols];

            r = (float)((float)((float)HighResMap.us_Scale / M44_SensorFusionAndMappingService.i_MapRes) / 4);
            float [] c = new float[16];

            for (int i = 0; i < HighResMap.us_Nrows; i++)
            {
                for (int j = 0; j < HighResMap.us_Ncols; j++)
                {
                    i_CellPos[0] = i;
                    i_CellPos[1] = j;
                    /*i and j go through each cell*/
                    lim = M44_BasicClass.Cell2Coordinates_High(HighResMap,i_CellPos);

                    c[0] = 0; c[1] = 0; c[2] = 0; c[3] = 0;
                    c[4] = 0; c[5] = 0; c[6] = 0; c[7] = 0;
                    c[8] = 0; c[9] = 0; c[10] = 0; c[11] = 0;
                    c[12] = 0; c[13] = 0; c[14] = 0; c[15] = 0; 

                    /*k goes through the number of points of each cell*/
                    if (Cell_List[i, j] != null)
                    {          
                        foreach (cl_Pos3D pos in Cell_List[i, j])
                        {
                            
                            x = pos.f_x;
                            //y = pos.f_y;
                            y = pos.f_z;

                            #region cuadrants
                            /*cuadrant 1*/

                            if ((x >= lim[0]) && (x <= lim[0] + r) && (y >= lim[1] + 3 * r) && (y <= lim[1] + 4 * r))
                            {
                                
                                c[0] = 1;
                            }
                            /*cuadrant 2*/
                            else if ((x >= lim[0] + r) && (x <= lim[0] + 2 * r) && (y >= lim[1] + 3 * r) && (y <= lim[1] + 4 * r))
                            {
                                
                                c[1] = 1;
                            }
                            /*cuadrant 3*/
                            else if ((x >= lim[0] + 2 * r) && (x <= lim[0] + 3 * r) && (y >= lim[1] + 3 * r) && (y <= lim[1] + 4 * r))
                            {
                                c[2] = 1;
                                
                            }
                            /*cuadrant 4*/
                            else if ((x >= lim[0] + 3 * r) && (x <= lim[0] + 4 * r) && (y >= lim[1] + 3 * r) && (y <= lim[1] + 4 * r))
                            {
                                c[3] = 1;
                                
                            }
                            /*cuadrant 5*/
                            else if ((x >= lim[0]) && (x <= lim[0] + r) && (y >= lim[1] + 2 * r) && (y <= lim[1] + 3 * r))
                            {
                                c[4] = 1;
                                
                            }
                            /*cuadrant 6*/
                            else if ((x >= lim[0] + r) && (x <= lim[0] + 2 * r) && (y >= lim[1] + 2 * r) && (y < lim[1] + 3 * r))
                            {
                                c[5] = 1;
                                
                            }
                            /*cuadrant 7*/
                            else if ((x >= lim[0] + 2 * r) && (x <= lim[0] + 3 * r) && (y >= lim[1] + 2 * r) && (y <= lim[1] + 3 * r))
                            {
                                c[6] = 1;
                                
                            }
                            /*cuadrant 8*/
                            else if ((x >= lim[0] + 3 * r) && (x <= lim[0] + 4 * r) && (y >= lim[1] + 2 * r) && (y <= lim[1] + 3 * r))
                            {
                                c[7] = 1;
                               
                            }
                            /*cuadrant 9*/
                            else if ((x >= lim[0]) && (x <= lim[0] + r) && (y >= lim[1] + r) && (y <= lim[1] + 2 * r))
                            {
                                c[8] = 1;
                                
                            }
                            /*cuadrant 10*/
                            else if ((x >= lim[0] + r) && (x <= lim[0] + 2 * r) && (y >= lim[1] + r) && (y <= lim[1] + 2 * r))
                            {
                                c[9] = 1;
                                
                            }
                            /*cuadrant 11*/
                            else if ((x >= lim[0] + 2 * r) && (x <= lim[0] + 3 * r) && (y >= lim[1] + r) && (y <= lim[1] + 2 * r))
                            {
                                c[10] = 1;
                                
                            }
                            /*cuadrant 12*/
                            else if ((x >= lim[0] + 3 * r) && (x <= lim[0] + 4 * r) && (y >= lim[1] + r) && (y <= lim[1] + 2 * r))
                            {
                                c[11] = 1;
                               
                            }
                            /*cuadrant 13*/
                            else if ((x >= lim[0]) && (x <= lim[0] + r) && (y >= lim[1]) && (y <= lim[1] + r))
                            {
                                c[12] = 1;
                                
                            }
                            /*cuadrant 14*/
                            else if ((x >= lim[0] + r) && (x <= lim[0] + 2 * r) && (y >= lim[1]) && (y <= lim[1] + r))
                            {
                                c[13] = 1;
                                
                            }
                            /*cuadrant 15*/
                            else if ((x >= lim[0] + 2 * r) && (x <= lim[0] + 3 * r) && (y >= lim[1]) && (y <= lim[1] + r))
                            {
                                c[14] = 1;
                                
                            }
                            /*cuadrant 16*/
                            else if ((x >= lim[0] + 3 * r) && (x <= lim[0] + 4 * r) && (y >= lim[1]) && (y <= lim[1] + r))
                            {
                                c[15] = 1;
                                
                            }
                            #endregion
                        }

                    }

                    /*Now the cuadrants are counted*/
                    for (int counter = 0; counter < 16; counter++)
                    {
                        if (c[counter] == 1)
                        {
                            Quads_Matrix[i, j] = (byte)(Quads_Matrix[i, j] + 1);
                        }
                    }
                }
            }
            return Quads_Matrix;
        }

    }
}

/* ------------------------------------------------------------------------- */
/*
 * RIGHT OF USE. This document may neither be passed on to third parties or
 * reproduced nor its contents utilized or divulged without the expressed
 * prior permission of the EUROPEAN DEFENCE AGENCY, or any national government
 * having rights on it. In case of contravention, the offender shall be
 * liable for damages.
 *
 * ATTENTION! DEFENCE MATERIAL. This document may contain data which is subject
 * to export control. For the export of this data an export license is
 * required.
 *
 * COMPANY PROPRIETARY. This document contains proprietary information and
 * may only be used by the recipient for the prescribed purposes and may
 * neither be reproduced in any form nor the document itself or its content
 * divulged to third parties without our expressed prior written permission.
 *
 * COPYRIGHT (C) Diehl BGT Defence GmbH & Co. KG; 2008; All rights reserved; 
 *
 * DISTRIBUTION AND USAGE RIGHTS: Restricted to the NETWORKED MULTI-ROBOT
 * SYSTEMS Project Consortium, participation governments, prime contractor,
 * subcontractors and vendors in accordance with contract / subcontract
 * clauses and / or other specified written agreements.
 */
/* ------------------------------------------------------------------------- */
